#include <core/synced_current_covariance.h>
#include <boost/thread/locks.hpp>

using boost::unique_lock;

SyncedCurrentCovariance::SyncedCurrentCovariance() {
  current_covariance_ = gsl_matrix_alloc(3, 3);
}

SyncedCurrentCovariance::~SyncedCurrentCovariance() {
  gsl_matrix_free(current_covariance_);
}

void SyncedCurrentCovariance::set_current_covariance(const gsl_matrix *current_covariance) {
  unique_lock<mutex> lock(mutex_);
  gsl_matrix_memcpy(current_covariance_, current_covariance);
}

void SyncedCurrentCovariance::current_covariance(gsl_matrix *current_covariance) {
  unique_lock<mutex> lock(mutex_);
  gsl_matrix_memcpy(current_covariance, current_covariance_);
}
